//
//  Plane.cpp
//  ProjektMagisterski
//
//  Created by Marcin Stramowski on 11.02.2017.
//


#include <pcl/features/moment_of_inertia_estimation.h>
#include <constants.h>
#include <Plane.h>

Plane::Plane() {}

// 直接给出平面的参数
Plane::Plane(vector<float> coefficients, const PointCloud::Ptr& points)
{
    this->octree = new OctreeManager(0.05);
    octree->updateByPointCloud(points);
    cloud = octree->getPointCloudPtr();
    computeMean();

    A = coefficients[0];
    B = coefficients[1];
    C = coefficients[2];
    D = -coefficients[3];
    planeNormalVec(0) = A;
    planeNormalVec(1) = B;
    planeNormalVec(2) = C;
}

// Plane类中用Ax+By+Cy+D来存储平面
Plane::Plane(double D, const Vector3d &planeNormalVec) : D(D), planeNormalVec(planeNormalVec) {
    A = planeNormalVec(0);
    B = planeNormalVec(1);
    C = planeNormalVec(2);
}

// quad是存的{A,B,C,D}，因此 plane.D = -quad.D
Plane::Plane(Eigen::Quaterniond quaternion){
    Eigen::Vector3d normVec(quaternion.x(), quaternion.y(), quaternion.z());
    double norm = normVec.norm();
    normVec.normalize();
    A = normVec(0);
    B = normVec(1);
    C = normVec(2);
    D = -quaternion.w() / norm;
    planeNormalVec = normVec;
}

Plane::Plane(const Vector3d& _normal, const Vector3d& _mean, const PointCloud::Ptr& cloud)
{
    this->octree = new OctreeManager(0.05);
    octree->updateByPointCloud(cloud);

    this->meanPoint = _mean;

    Eigen::Vector3d planeParameters = _normal;
    planeParameters.normalize();
    A = planeParameters(0);
    B = planeParameters(1);
    C = planeParameters(2);
    
    Vector3d point1 = this->getMeanPoint();
    D = A * point1(0) + B * point1(1) + C * point1(2);
    valid = true;
    planeNormalVec(0) = A;
    planeNormalVec(1) = B;
    planeNormalVec(2) = C;
}

Plane::Plane(Vector3d _normalVec, Vector3d _mean, vector<Point3D> _points)
{
    this->points = _points;
    this->meanPoint = _mean;
    
    Eigen::Vector3d planeParameters = _normalVec;
    planeParameters.normalize();
    A = planeParameters(0);
    B = planeParameters(1);
    C = planeParameters(2);
    
    Vector3d point1 = this->getMeanPoint();
    D = A * point1(0) + B * point1(1) + C * point1(2);
    valid = true;
    planeNormalVec(0) = A;
    planeNormalVec(1) = B;
    planeNormalVec(2) = C;
}

Plane::Plane(const Vector3d &point1, const Vector3d &point2, const Vector3d &point3, Vector3d _meanPoint) {
    computePlaneEquation(point1, point2, point3);
    this->meanPoint = _meanPoint;
}

void Plane::computePlaneEquation(const Vector3d &point1, const Vector3d &point2, const Vector3d &point3) {
    Eigen::Vector3d v = point1 - point2;
    Eigen::Vector3d w = point1 - point3;
    Eigen::Vector3d planeParameters = v.cross(w);
    planeParameters.normalize();
    A = planeParameters(0);
    B = planeParameters(1);
    C = planeParameters(2);
    D = A * point1(0) + B * point1(1) + C * point1(2);
    valid = true;
    planeNormalVec(0) = A;
    planeNormalVec(1) = B;
    planeNormalVec(2) = C;

    //this->meanPoint = (point1+point2+point3)/3;
}


void Plane::computeNormalDirection()
{
    Eigen::Vector3d meanDirection = meanPoint.normalized();
    double angle = acos(planeNormalVec.dot(meanDirection)) * 180.0 / M_PI;
    if(angle < 90)
    {
        planeNormalVec = -planeNormalVec;
        A = -A;
        B = -B;
        C = -C;
        D = -D;
    }
}

void Plane::computeNormalVecDirection(){
    Vector3d cameraAxisVec(0.0f, 0.0f, -1.0f);
    planeNormalVec.normalize();
    double normalVecCameraAxisAngle = acos(planeNormalVec.dot(cameraAxisVec)) * 180.0 / M_PI;

    if(!(normalVecCameraAxisAngle > 85 && normalVecCameraAxisAngle < 95)){
        if (normalVecCameraAxisAngle > 90) {
            planeNormalVec = -planeNormalVec;
            A = -A;
            B = -B;
            C = -C;
            D = -D;
        }
    } else {
        if(points.size() != 0){
            Vector3d cameraToPlaneVec = points.at((points.size() - 1) / 2).position;
            cameraToPlaneVec.normalize();
            double angle = acos(planeNormalVec.dot(cameraToPlaneVec)) * 180.0 / M_PI;
            if(angle < 90) {
                planeNormalVec = -planeNormalVec;
                A = -A;
                B = -B;
                C = -C;
                D = -D;
            }
        }
    }
}

void Plane::insertPointCloud(const PointCloud::Ptr& points)
{
    octree->addPointCloud(points);
}

void Plane::transform(const Eigen::Matrix4d& t)
{
    transformParas(t);
    pcl::transformPointCloud(*cloud, *cloud, t);
}

// 注意这里转换时，D存的仍然是Ax+By+Cz而不是相反数
void Plane::transformParas(const Eigen::Matrix4d& t)
{
    // 要先转换center, 因为平面参数D的计算要依赖于center
    Eigen::Vector4d meanPoint4;
    meanPoint4.topRightCorner(3, 1) = meanPoint;
    meanPoint4(3) = 1;
    meanPoint = (t*meanPoint4).topRightCorner(3,1);

    // 转换normal
    auto rotMatrix = t.topLeftCorner(3, 3);
    planeNormalVec = rotMatrix * planeNormalVec;
    A = planeNormalVec(0);
    B = planeNormalVec(1);
    C = planeNormalVec(2);
    D = A*meanPoint(0) + B*meanPoint(1) + C*meanPoint(2);
}

// 用posOrient将平面变化到全局坐标系下
Plane Plane::getPlaneSeenFromGlobalCamera(PosOrient &posOrient) {
    Eigen::Matrix4d t = posOrient.toTransformMatrix();
    return getPlaneSeenFromGlobalCamera(t);
}

Plane Plane::getPlaneSeenFromGlobalCamera(Eigen::Matrix4d &posMatrix){
    //复制自原平面
    Plane plane = *this; 
    plane.transformPlaneToGlobal(posMatrix);
    return plane;
}

void Plane::transformPlaneToGlobal(Eigen::Matrix4d &posMatrix){
    this->transformMeanToGlobal(posMatrix);
    this->transformPointsToGlobal(posMatrix);
    this->transformParametersToGlobal(posMatrix);
}

void Plane::transformParametersToGlobal(Eigen::Matrix4d &posMatrix){
    Matrix3d rotMatrix = posMatrix.topLeftCorner(3, 3);
    Vector3d norm = rotMatrix * getPlaneNormalVec();
    A = norm(0);
    B = norm(1);
    C = norm(2);
    D = A*meanPoint(0) + B*meanPoint(1) + C*meanPoint(2);
    planeNormalVec = norm;
}

void Plane::transformMeanToGlobal(Eigen::Matrix4d &t){
    // 齐次坐标
    Vector4d meanPoint4; 
    meanPoint4.topRightCorner(3, 1) = meanPoint;
    meanPoint4(3) = 1;
    meanPoint = (t*meanPoint4).topRightCorner(3,1);
}

void Plane::transformPointsToGlobal(Eigen::Matrix4d &t) {
    PosOrient pos(t);
    transformPointsToGlobal(pos);
}

void Plane::transformPointsToGlobal(PosOrient &posOrient) {
    points3D.setPoints(points);
    points3D.transform(posOrient);
    points = points3D.getPoints3D();
}

bool Plane::getPositiveFromPoint(Vector3d point)
{
    return A * point(0) + B * point(1) + C * point(2) - D > 0;
}

double Plane::getDistanceFromPoint(Vector3d point) const {
    return std::abs(A * point(0) + B * point(1) + C * point(2) - D)
           / sqrt(pow(A, 2.0) + pow(B, 2.0) + pow(C, 2.0));
}

double Plane::getDistanceFromPoint(const Vector3d &point){
    return std::abs(A * point(0) + B * point(1) + C * point(2) - D)
           / sqrt(pow(A, 2.0) + pow(B, 2.0) + pow(C, 2.0));
}

double Plane::getMeanDistanceFromPointCloud(const vector<Point3D> &points){
    double sum = 0;
    for(auto p : points){
        sum += this->getDistanceFromPoint(p.position);
    }
    return sum /= points.size();
}

void Plane::setPoints(PointCloud::Ptr &cloud)
{
    Point3DCloud points;
    points.fromPointCloud(cloud);
    setPoints(points.getPoints3D());
}

void Plane::clearPoints() {
    points.clear();
}

void Plane::clearAllPointsExceptSome() {
    random_device rd;
    mt19937 rng(rd());
    uniform_int_distribution<unsigned int> pointIndex(0, (unsigned int)(points.size() - 1));
    vector<Point3D> randomPoints;
    for (int i = 0; i < NUMBER_OF_POINTS_TO_COUNT_DISTANCE; ++i) {
        randomPoints.push_back(points[pointIndex(rng)]);
    }
    points.clear();
    points.insert(points.end(), randomPoints.begin(), randomPoints.end());
}

unsigned int Plane::getNumberOfPoints() const {
    return (unsigned int) points.size();
}

double Plane::getAngleBetweenTwoPlanes(const Plane &plane) const{
    Eigen::Vector3d planeNormalVec = plane.getPlaneNormalVec();

    double angleCos = (this->planeNormalVec(0) * plane.getPlaneNormalVec()(0) +
                        this->planeNormalVec(1) * plane.getPlaneNormalVec()(1) +
                        this->planeNormalVec(2) * plane.getPlaneNormalVec()(2)) /
                        this->planeNormalVec.norm() / planeNormalVec.norm();
    if (angleCos < -1) angleCos = -1.0;
    if (angleCos > 1) angleCos = 1.0;
    return acos(angleCos) * 180.0 / (double) M_PI;
}

double Plane::getDistanceFromPlane(Plane &plane){
    if (this->getPoints().size() == 0 || plane.getPoints().size() == 0)
            return -1;

        random_device rd;
        mt19937 rng(rd());

        double distance = 0;
        vector<Point3D> pointsVec = plane.getPoints();

        uniform_int_distribution<unsigned int> pointIndex(0, plane.getNumberOfPoints() - 1);
        for (int i = 0; i < NUMBER_OF_POINTS_TO_COUNT_DISTANCE; ++i)
        {
            Vector3d randomPointOnSecondPlane = pointsVec[pointIndex(rng)].position;
            distance += this->getDistanceFromPoint(randomPointOnSecondPlane);
        }

        return distance / NUMBER_OF_POINTS_TO_COUNT_DISTANCE;
}

void Plane::insertPoints(const vector<Point3D>& points) {
    this->points.insert(this->points.end(), points.begin(), points.end());
}


Vector3d Plane::computeMean(const PointCloud::Ptr &points)
{
    Vector3d mean(0,0,0);
    for(auto &p : points->points)
    {
        mean[0] += p.x;
        mean[1] += p.y;
        mean[2] += p.z;
    }
    mean /= (double)points->points.size();
    return mean;
}

void Plane::computeMean()
{
    this->meanPoint = computeMean(this->cloud);
}

// Vector3d Plane::computeMean(const vector<Point3D> &pointsVector) 
// {
//         Vector3d mean(0, 0, 0);
//         for (auto &vector : pointsVector) {
//             mean += vector.position;
//         }
//         mean /= (double) pointsVector.size();
//         meanPoint = mean;
//         return mean;
// }

void Plane::computeAABB()
{
    pcl::MomentOfInertiaEstimation <PointType> feature_extractor;
    PointType min_point_AABB, max_point_AABB;
    feature_extractor.setInputCloud (cloud);
    feature_extractor.compute ();
    feature_extractor.getAABB (min_point_AABB, max_point_AABB);
    aabb = make_pair(min_point_AABB, max_point_AABB);
}

void Plane::updatePlaneParameters(Plane &plane) {
    A = plane.getA();
    B = plane.getB();
    C = plane.getC();
    D = plane.getD();
    planeNormalVec = plane.getPlaneNormalVec();
}

float Plane::getMeanDistanceBetweenTwoPlanes(const Plane& plane)
{
    return (meanPoint - plane.getMeanPoint()).norm();
}

bool Plane::isCloseTo(const Plane& plane)
{
    return this->getMeanDistanceBetweenTwoPlanes(plane) < THRESHOLD_BETWEEN_PLANE_CENTER;
}

bool Plane::isEqualTo(Plane &plane)
{
    return this->getAngleBetweenTwoPlanes(plane) < MAX_ANGLE_BETWEEN_EQUAL_PlANES
           && abs(D - plane.getD()) < D_THRESHOLD_BETWEEN_EQUAL_PLANES;
}

bool Plane::isMatchWith(Plane &plane){
    return this->getAngleBetweenTwoPlanes(plane) < MAX_ANGLE_BETWEEN_PLANES_GLOBAL_MAP
           && abs(D - plane.getD()) < D_THRESHOLD;
}
